#include "motion.h"
#include "../config/config.h"
#include "../led/led.h"
#include <Servo.h>
Servo servo;
LED led;

// 初始化电机驱动各个引脚
void Motion::init() {
  led.init();
  pinMode(leftMotor1, OUTPUT);
  pinMode(leftMotor2, OUTPUT);
  pinMode(leftPWM, OUTPUT);

  pinMode(rightMotor1, OUTPUT);
  pinMode(rightMotor2, OUTPUT);
  pinMode(rightPWM, OUTPUT);

  servo.attach(servoPin);
  servo.write(98);
}

// 设置小车速度
void Motion::speed(int LevelSpeed) {
  // 设置运动的速度
  analogWrite(leftPWM, LevelSpeed);
  analogWrite(rightPWM, LevelSpeed);
}

// 运动函数
void Motion::run(int cmd)
{
  switch (cmd)
  {
  case FORWARD:
    digitalWrite(leftMotor1, LOW);
    digitalWrite(leftMotor2, HIGH);
    digitalWrite(rightMotor1, LOW);
    digitalWrite(rightMotor2, HIGH);
    led.TurnLeftClose();
    led.TurnRightClose();
    break;
  case BACKWARD:
    digitalWrite(leftMotor1, HIGH);
    digitalWrite(leftMotor2, LOW);
    digitalWrite(rightMotor1, HIGH);
    digitalWrite(rightMotor2, LOW);
    led.TurnRight();
    led.TurnLeft();
    break;
  case TURNLEFT:
    led.TurnLeft();
    led.TurnRightClose();
    servo.write(40);
    break;
  case TURNRIGHT:
    led.TurnRight();
    led.TurnLeftClose();
    servo.write(140);
    break;
  case STRAIGHT:
    led.TurnLeftClose();
    led.TurnRightClose();
    servo.write(100);
    break;
  default:
    led.TurnLeftClose();
    led.TurnRightClose();
    digitalWrite(leftMotor1, LOW);
    digitalWrite(leftMotor2, LOW);
    digitalWrite(rightMotor1, LOW);
    digitalWrite(rightMotor2, LOW);
  }
}

// 控制舵机转向一定角度
void Motion::turn(int nums) {
  servo.write(nums);
}

// 开启双闪
void Motion::open() {
  led.open();
}

// 开启双闪
void Motion::close() {
  led.close();
}

// 后退并前进
void Motion::left() {
  // backOff();  
  speed(100);
  delay(100);
  run(1);
}

// 后退并前进
void Motion::right() {
  // backOff();  
  speed(100);
  delay(100);
  run(1);
}

// 前进
void Motion::straight() {
  // 设置电机速度
  speed(100);
  // 直行
  servo.write(90);
  delay(100);
  run(1);
}

// 后退
void Motion::backOff() {
  speed(180);
  run(2);
  delay(1000);
  run(0);
  delay(100);
}